import os
import chardet
import numpy as np
import open3d as o3d


def detect_encoding(file_path):
    with open(file_path, 'rb') as f:
        result = chardet.detect(f.read())
    return result['encoding']


def points2pcd(PCD_FILE_PATH, points):
    """
    点云数据保存到pcd文件
    :param PCD_FILE_PATH: 保存的文件地址
    :param points: 需要保存的点云数据
    :return:
    """
    # 存放路径
    if os.path.exists(PCD_FILE_PATH):
        os.remove(PCD_FILE_PATH)
    # 写文件句柄
    handle = open(PCD_FILE_PATH, 'a')
    # 得到点云点数
    point_num = points.shape[0]
    # pcd头部（重要）
    handle.write(
        '# .PCD v0.7 - Point Cloud Data file format\nVERSION 0.7\nFIELDS x y z\nSIZE 4 4 4\nTYPE F F F\nCOUNT 1 1 1')
    string = '\nWIDTH ' + str(point_num)
    handle.write(string)
    handle.write('\nHEIGHT 1\nVIEWPOINT 0 0 0 1 0 0 0')
    string = '\nPOINTS ' + str(point_num)
    handle.write(string)
    handle.write('\nDATA ascii')
    # 依次写入点
    for i in range(point_num):
        string = '\n' + str(points[i, 0]) + ' ' + str(points[i, 1]) + ' ' + str(points[i, 2])
        handle.write(string)
    handle.close()


def calculate_direction_vector(a, b):
    """
    根据给定的角度a和b计算射线的方向向量。
    :param a: 与Z轴正方向之间的夹角（弧度）
    :param b: 绕Z轴的偏转角（弧度）
    :return: 方向向量
    """
    dx = np.sin(a) * np.sin(b)
    dy = np.sin(a) * np.cos(b)
    dz = np.cos(a)
    return np.array([dx, dy, dz])


def is_point_within_distance(point, direction_vector, max_distance=5):
    """
    判断点到射线的距离是否小于给定的最大距离。
    :param point: 点的坐标，类型为np.array([x, y, z])
    :param direction_vector: 射线的方向向量
    :param max_distance: 最大允许距离
    :return: 是否满足条件的布尔值
    """
    proj_distance = np.dot(point, direction_vector) / np.linalg.norm(direction_vector)
    cross_product = np.cross(point, direction_vector)
    distance = np.linalg.norm(cross_product)
    return distance, proj_distance


def calculate_angle(origin_position, light_pole_position, radius):
    """
    计算路灯杆遮挡角度范围
    :param origin_position: 雷达点位坐标
    :param light_pole_position: 路灯杆点位坐标
    :param radius: 路灯杆杆半径
    :return: 路灯杆遮挡的角度范围
    """
    dy = light_pole_position[1] - origin_position[1]
    dx = light_pole_position[0] - origin_position[0]
    angle = 90 - np.degrees(np.arctan2(dy, dx))
    pole_dis = np.linalg.norm(origin_position - light_pole_position)
    ag = np.degrees(np.arcsin(radius / pole_dis))
    anglee = [angle - ag, angle + ag]
    for i in range(2):
        if anglee[i] < 0:
            anglee[i] += 360
        elif anglee[i] > 360:
            anglee[i] -= 360
    return anglee


def delete_file(path):
    """
    删除文件
    :param path: 文件路径
    :return:
    """
    if os.path.exists(path):
        try:
            os.remove(path)
            print(f"文件 {path} 已被删除。")
        except OSError as e:
            print(f"删除文件 {path} 时发生错误: {e.strerror}")
    else:
        print(f"文件 {path} 不存在。")


def show_pcd(points):
    # save_path = r'D:\GJ\0424\all32_6.pcd'
    # 创建点云对象
    filtered_pcd = o3d.geometry.PointCloud()
    filtered_pcd.points = o3d.utility.Vector3dVector(points)
    # # 可视化结果
    o3d.visualization.draw_geometries([filtered_pcd])
    # o3d.io.write_point_cloud(save_path, filtered_pcd, write_ascii=True)


def can_meage(list1, list2):
    start1, end1 = list1
    start2, end2 = list2
    if start1 < start2:
        if end1 > start2:
            if end1 > end2:
                return list1
            else:
                return [start1, end2]
        else:
            return []
    else:
        if end2 > start1:
            if end2 > end1:
                return list2
            else:
                return [start2, end1]
        else:
            return []


def find_frist_pcd_or_ply(directory):
    for filename in os.listdir(directory):
        if filename.endswith('.pcd') or filename.endswith('.ply'):
            filepath = os.path.join(directory, filename)
            return filepath

    return None
